package simulator.entities;

import java.awt.Graphics;

import simulator.drawengine.Drawable;
import simulator.enums.Direction;
import simulator.enums.EntityType;
import simulator.enums.LightState;
import simulator.physengine.PhysObject;
import simulator.types.Lane;
import simulator.types.Vector2f;

public abstract class BaseVehicle extends PhysObject implements Drawable
{
	public static final float		CHECKPOINT_ACCURACY		= 0.7f;
	
	/*used for calculations. Values inside only valid when set and method has not yet been exited */
	private static final Vector2f	TempVector				= new Vector2f();

	protected boolean				_checkFarLoop			= true;

	protected TrafficLane			_beginLane;
	protected TrafficLane			_destinationLane;

	protected float					_maxSpeed				= 18;				//10 units/sec
	protected float					_vehicleAcceleration	= 6;
	protected float					_brakePower				= 12;

	/* length of this vehicle */
	protected float					_vehicleLength			= 2;
	
	/* preferred distance in meters between front of current vehicle and rear of vehicle in front */
	protected float					_preferredSpacing		= 1;

	protected boolean				_driving;
	protected boolean				_avoidance;
	
	protected BaseVehicle			_front;									//vehicle in front of this vehicle

	protected boolean				_onApproach;
	protected boolean				_onExit;

	protected boolean				_farLoopTriggered		= false;
	protected boolean				_nearLoopTriggered		= false;

	public BaseVehicle(TrafficLane spawn, TrafficLane dest)
	{
		_position.copy(spawn._spawnPosition);

		_driving = true;
		_beginLane = spawn;
		_destinationLane = dest;

		_onApproach = true;
		_onExit = false;
	}

	public abstract EntityType getType();

	public TrafficLane getSpawn()
	{
		return _beginLane;
	}

	public TrafficLane getDestination()
	{
		return _destinationLane;
	}

	@Override
	public void update(float dt)
	{
		if (_onApproach)
		{
			updateApproach();
		}
		else if (_onExit)
		{
			updateExit();
		}
		else
		{
			updateCrossing();
		}

		updateAcceleration();

		//System.out.println("car position= " + _position.toString() + " heading=" + _heading + " velocity=" + _velocity + " driving=" + _driving +  " avoidance=" + _avoidance + " approach=" + _onApproach + " exit=" + _onExit + " light=" + _beginLane._light.getState().getSpecString());
	}

	protected void updateApproach()
	{
		_driving = true;

		//calculate heading
		TempVector.copy(_beginLane._holdPosition);
		TempVector.sub(_position);

		_heading = (float) Math.atan2(TempVector.y, TempVector.x);

		//check far loop triggering
		if (_checkFarLoop)
		{
			boolean inrange = _position.getDistance(_beginLane._farLoopPosition) < CHECKPOINT_ACCURACY;
			if (inrange && !_farLoopTriggered)
			{
				_beginLane.farLoopEntered(this);
				_farLoopTriggered = true;
			}
			else if (!inrange && _farLoopTriggered)
			{
				_beginLane.farLoopExited(this);
				_farLoopTriggered = false;
			}
		}

		//check near loop triggering
		boolean inrange = _position.getDistance(_beginLane._nearLoopPosition) < CHECKPOINT_ACCURACY;
		if (inrange && !_nearLoopTriggered && ((getType() != EntityType.BIKE && getType() != EntityType.PED) || lightStatusStops()))
		{
			_beginLane.nearLoopEntered(this);
			_nearLoopTriggered = true;
		}
		else if (!inrange && _nearLoopTriggered)
		{
			_beginLane.nearLoopExited(this);
			_nearLoopTriggered = false;
		}

		//collision avoidance
		_avoidance = collisionAvoidance();
		
		//traffic light check
		boolean couldStop = (getStoppingDistance() > _position.getDistance(_beginLane._holdPosition) - CHECKPOINT_ACCURACY) && (getStoppingDistance() < _position.getDistance(_beginLane._holdPosition) + CHECKPOINT_ACCURACY); //check if it is sensible to slow down at this point (braking now will cause the vehicle to end up somewhere near the holding position)
		boolean onHold = (_position.getDistance(_beginLane._holdPosition) < CHECKPOINT_ACCURACY && getStoppingDistance() < 2 * CHECKPOINT_ACCURACY); //check if the vehicle is already idling at hold position (this stops oscillation around holding position)

		if ((couldStop || onHold) && lightStatusStops())
		{
			_driving = false;
		}
		else if (_position.getDistance(_beginLane._holdPosition) < CHECKPOINT_ACCURACY)
		{
			_onApproach = false;
		}

		//old code
		/*
		//check if this vehicle should start stopping in order to not collide with vehicle in front
		if (_front != null)
		{
			if (_driving)
			{
				//check if this vehicle should stop now in order to not collide with vehicle in front
				if (_position.getDistance(_front._position) <= (getStoppingDistance() + _vehicleLength + 0.5 - _front.getStoppingDistance()))
				{
					_driving = false;
				}
			}
			else if (_front._driving && !_driving)
			{
				if (_beginLane._light.getState().canContinue())
				{
					_driving = true;
				}
				else
				{
					_driving = false;
				}
			}
		}
		else
		{
			if (_beginLane._light.getState().canContinue())
			{
				_driving = true;
				if (_position.getDistance(_beginLane._holdPosition) < CHECKPOINT_ACCURACY)
				{
					_onApproach = false;
				}
			}
			else
			{
				if(getStoppingDistance() > _position.getDistance(_beginLane._holdPosition) || _position.getDistance(_beginLane._holdPosition) < CHECKPOINT_ACCURACY)
				{
					_driving = false;
				}
				else
				{
					_driving = true;
				}
			}
		}
		*/
	}
	
	protected boolean collisionAvoidance()
	{
		if (_front != null)
		{
			//motion-based estimation
			if (_position.getDistance(_front._position) <= (getStoppingDistance() + _vehicleLength + _preferredSpacing - _front.getStoppingDistance()))
			{
				_driving = false;
				return true;
			}
			//static check
			else if (_position.getDistance(_front._position) <= _vehicleLength + _preferredSpacing)
			{
				_driving = false;
				return true;
			}
		}
		
		return false;
	}

	protected boolean lightStatusStops()
	{
		switch (_beginLane._light._state)
		{
			case ALL:
			case OFF:
			case BLINK:
			case GREEN:
				return false;

			case BLINK_ERR:
			case ORANGE:
			case RED:
				return true;

			case LEFT:
				return _destinationLane._lane.getDirection() != Direction.getLeft(_beginLane._lane.getDirection());

			case RIGHT:
				return _destinationLane._lane.getDirection() != Direction.getRight(_beginLane._lane.getDirection());

			case STRAIGHT:
				return _destinationLane._lane.getDirection() != Direction.getStraight(_beginLane._lane.getDirection());

			default:
				return true;
		}
	}

	protected void updateCrossing()
	{
		if(_nearLoopTriggered)
		{
			_nearLoopTriggered = false;
			_beginLane.nearLoopExited(this);
		}
		
		//for now, just head in straight line to exit
		_driving = true;

		if (_position.getDistance(_destinationLane._holdPosition) < CHECKPOINT_ACCURACY)
		{
			_onExit = true;
			updateExit();
			return;
		}

		//calculate heading
		TempVector.copy(_destinationLane._holdPosition);
		TempVector.sub(_position);

		_heading = (float) Math.atan2(TempVector.y, TempVector.x);

		collisionAvoidance();
	}

	protected void updateExit()
	{
		//for now, drive straight for lane exit
		_driving = true;

		//calculate heading
		TempVector.copy(_destinationLane._spawnPosition);
		TempVector.sub(_position);

		_heading = (float) Math.atan2(TempVector.y, TempVector.x);

		collisionAvoidance();

		if (_position.getDistance(_destinationLane._spawnPosition) < CHECKPOINT_ACCURACY)
		{
			_position.set(999999);
			_front = null;
			_destinationLane._parent.detachVehicle(this);
			return;
		}
	}

	protected void updateAcceleration()
	{
		//set the correct acceleration for the current situation
		if (_driving)
		{
			if (_velocity < _maxSpeed)
				_acceleration = _vehicleAcceleration;
			else
			{
				_velocity = _maxSpeed;
				_acceleration = 0;
			}
		}
		else
		{
			if (_velocity > 0)
			{
				_acceleration = -_brakePower;
			}
			else
			{
				_velocity = 0;
				_acceleration = 0;
			}
		}
	}

	public void setFront(BaseVehicle front)
	{
		_front = front;
	}

	protected float getStoppingDistance()
	{
		if (_velocity < 0.01) return 0;

		float timeToStop = _velocity / _brakePower;
		return (_velocity / 2) * timeToStop * 1.02f;
	}
}